#include "Arduino.h"
#include "tinyOSC.h"
#include "WString.h"

tinyOSC::tinyOSC(void) {
}

void tinyOSC::begin(Stream &theUart) {
  uart = &theUart;
  _nill = 0;
  _zero = 0;
  _parseMode = 0;
  _flushCtr = 0;
  term_char = 0xFF;
  _start = '/';
  _wordSize = 4;
  _nillStr = "";
}

void tinyOSC::begin(Stream &theUart, byte end_char) {
  begin(theUart);
  term_char = end_char;
}


void tinyOSC::_init(void) {
  _parseMode = 1;
  containerList = String(_start);
  method = _nillStr;
  num_data = _zero;
  dataString = _nillStr;
  _dataCtr = _zero;
}

boolean tinyOSC::available(void) {
  char inChar;
  boolean avl = false;
  boolean got_char = false;
  boolean working = true;
  
  while (working) {
    if (uart->available()) {
      inChar = uart->read();
      switch (_parseMode) {
        case 0: {
          if (inChar == _start) {
            _init();
          }
          break;
        }
        case 1: {
          if (inChar == _nill) {
            _parseMode = 2;
          } else if (inChar == _start) {
            containerList += method + inChar;
            method = _nillStr;
          } else if ((inChar > 0x1F) && (inChar < 0x7F)){
            method += inChar;
          } else {
            _parseMode = _zero;
          }
          break;
        }
        case 2: {
          if (inChar == ',') {
            _parseMode = 3;
            _flushCtr = 1;
          } else if (inChar == _start) {
            _init();
            _parseMode = 1;
          } else if (inChar != _nill) {
            _parseMode = _zero;
          }
          break;
        }
        case 3: {
          _flushCtr = (_flushCtr + 1) % _wordSize;
          if (inChar == _nill) {
            if (_dataCtr == _zero) {
              working = false;
              avl = true;
              _parseMode = 0;
            } else if (_flushCtr == _zero) {
              _parseMode = 4;
              num_data = _dataCtr;
              _dataCtr = _zero;
              _stackPtr = _zero;
              dataString = _nillStr;
            }
          } else if (inChar == _start) {
            _init();
          } else if ((inChar != 'b') && (inChar != 'f') && (inChar != 'i') && (inChar != 's') ) {
            _parseMode = _zero;
          } else if (_dataCtr < MAX_DATA_STACK) {
            dataType[_dataCtr++] = inChar;
          }
          break;
        }
        case 4: {
          _flushCtr = (_flushCtr + 1) % _wordSize;
          if (dataType[_dataCtr] == 's') {
            if (inChar == _nill) {
              if (_flushCtr == _zero) {_dataCtr++; }
            } else {
              dataString += inChar;
            }
          } else {
              _dataStack[_stackPtr++] = inChar;
              if (_flushCtr == _zero) {_dataCtr++; }
          }
          if (_dataCtr >= num_data) {
            _stackPtr = _zero;
            working = false;
            avl = true;
            _parseMode = _zero;
          }
          break;
        }
      }
    } else {
      working = false;
    }
  }
  return avl;
}

int tinyOSC::iPop(void) {
  int dataPtr = _stackPtr % _wordSize;
  int value = _zero;
  if (dataPtr < num_data) {
    switch (dataType[dataPtr]) {
      case 'i': {
        value = int(pop_long());
        break;
      }
      case 'f': {
        value = int(pop_float());
        break;
      }
    }
  }
  return value;
}

float tinyOSC::fPop(void) {
  int dataPtr = _stackPtr % _wordSize;
  float value = 0.0;
  if (dataPtr < num_data) {
    switch (dataType[dataPtr]) {
      case 'i': {
        value = float(pop_long());
        break;
      }
      case 'f': {
        value = pop_float();
        break;
      }
    }
  }
  return value;
}


float tinyOSC::pop_float(void) {
  union u_tag {
    byte b[4];
    float fval;
  } u;
  
  if (_stackPtr < MAX_DATA_START) {
    for (int i = _zero; i < _wordSize; i++) {
      u.b[3-i] = _dataStack[_stackPtr++];
    }
  } else {
    u.fval = 0.0;
  }

  return u.fval;
}

long tinyOSC::pop_long(void) {

long val = 0;
  if (_stackPtr < MAX_DATA_START) {
    for (int i = _zero; i < _wordSize; i++) {
      val = (val << 8) + long( _dataStack[_stackPtr++]);
    }
  }

  return val;
}

void tinyOSC::sendCmd(char *OSCstring) {
    
  _sendStr(OSCstring);

  uart->print(",");
  uart->write(_nill);
  uart->write(_nill);
  uart->write(_nill);

  _sendEnd();
  //uart->write(term_char);
}

void tinyOSC::sendString(char *OSCstring, String dataString) {
    
  _sendStr(OSCstring);

  uart->print(",s");
  uart->write(_nill);
  uart->write(_nill);
  uart->print(dataString);
  uart->write(_nill);
  int pad = (dataString.length() + 1) % _wordSize;
  if (pad > _zero) {
    for (int ctr = _zero; ctr < (_wordSize - pad); ctr++) {
      uart->write(_nill);
    }
  }
  _sendEnd();
  //uart->write(term_char);
}

void tinyOSC::sendMsg(char *OSCstring) {
  sendString(OSCstring, _msgString);
}

void tinyOSC::sendByte(char *OSCstring, byte OSCbyte) {

  _sendStr(OSCstring);

  uart->print(",i");
  for (int ctr = 0; ctr < 5; ctr++) uart->write(_nill);
  uart->write(OSCbyte);

  _sendEnd();
  //uart->write(term_char);
}

void tinyOSC::send2Byte(char *OSCstring, byte byte1, byte byte2) {

  _sendStr(OSCstring);

  uart->print(",ii");
  for (int ctr = 0; ctr < 4; ctr++) uart->write(_nill);
  uart->write(byte1);
  for (int ctr = 0; ctr < 3; ctr++) uart->write(_nill);
  uart->write(byte2);

  _sendEnd();
  //uart->write(term_char);
}


void tinyOSC::_sendStr(char *OSCstring) {
  int pad = _zero;
  //if (micros() < (_time + packet_delay)) {delayMicroseconds(packet_delay);}

  uart->print(_start);
  uart->print(OSCstring);
  uart->write(_nill);
  pad = (strlen(OSCstring) + 2) % _wordSize;
  if (pad > _zero) {
    for (int ctr = _zero; ctr < (_wordSize - pad); ctr++) {
      uart->write(_nill);
    }
  }
}

void tinyOSC::_sendEnd(void) {
  uart->write(_nill);
  uart->write(_nill);
  uart->write(_nill);
  uart->write(term_char);
  //_time = micros();
}

void tinyOSC::clearMsg(void) {
  _msgString = _nillStr;
}

void tinyOSC::addMsg(int msgInt) {
  _msgString += msgInt;
}

void tinyOSC::addMsg(String msgStr) {
  _msgString += msgStr;
}

void tinyOSC::addMsg(char msgChar) {
  _msgString += msgChar;
}

void tinyOSC::addMsg(char *msgChar) {
  _msgString += String(msgChar);
}

